#include <stdlib.h>
#include <string.h>
#include "qfilt.h"
 
#ifdef _MSC_VER
#define INLINE __inline
#else
#define INLINE inline
#endif

// indexes in SOS array of filter coefficients
#define B1 0
#define A1 1
#define A2 2

// index in W array of internal state values
#define W1 0
#define W2 1

// Initialize a new filter object from its coefficients
void qfilt_init(qfilt *f, const fixedp *sos, int sosFracbits, const fixedp *g, int gFracbits, fixedp *w, size_t L)
{
	f->sos = sos;
	f->sosFracbits = sosFracbits;
	f->g = g;
	f->gFracbits = gFracbits;
	f->w = w;
	f->L = L;

	qfilt_reset(f);
}

// Release all resources associated with the filter
void qfilt_free(qfilt *f)
{
	memset(f, 0, sizeof(*f));
}

static INLINE fixedp __qmul2(fixedp x, fixedp y, int shift)
{
	// multiply normally into 64 bits, then shift the decimal point and truncate
	return (fixedp)(((long long)x * (long long)y) >> shift);
}

// Compute output y for input x using Cascade Direct Form-II Transpose implementation
// See Proakis, Manolakis. "Digital Signal Processing" 4th Ed, pg 589
// AND 	http://www.mathworks.com/access/helpdesk/help/toolbox/signal/dfilt.df2tsos.html
fixedp qfilt_filter(qfilt *f, fixedp x)
{
	unsigned int i;
	for(i = 0; i < f->L; i++) {
		fixedp y;
		const fixedp *sos = &f->sos[3*i];
		fixedp *w = &f->w[2*i];

		// multiply by the scale value
		x = __qmul2(f->g[i], x, f->gFracbits);

		// y = x + w[W1];
		y = qadd(x, w[W1]);
		// w1 = sos[B1]*x - sos[A1]*y + w[W2];
		w[W1] = qadd(qsub(__qmul2(sos[B1], x, f->sosFracbits), __qmul2(sos[A1], y, f->sosFracbits)), w[W2]);		
		// w2 = x - sos[A2]*y;
		w[W2] = qsub(x, __qmul2(sos[A2], y, f->sosFracbits));

		x = y;
	}
	return x;
}

// Reset the state of the filter by zeroing out w
void qfilt_reset(qfilt *f)
{
	memset(f->w, 0, 2*f->L * sizeof(*f->w));
}
